/* ------------------------------------------------------------------------------
  File: CHR_GPS.c
  Author: CH Robotics
  Version: 1.0

  Description:

  Functions for processing and storing GPS data.

------------------------------------------------------------------------------ */
#include "../../include/sensors/CHR_GPS.h"
#include <string.h>
#include <stdio.h>


// Global data for parsing functions
GPS_Struct gGPSData;
char gGPSPacket[MAX_GPS_PACKET_SIZE];

/* ATTENTION NECESSITE SANS DOUTE DE LANCER LE MODULE WLAN !!!!! */

void read_buf_gps(GPS_Struct *GPS_data)
{
    FILE*fGPS;
    int nlines=0;
    /* lancer ou vérifier le script 1 du GPS : GPS_script1_file "/usr/sbin/lbsd" */
    /* lancer ou vérifier le script 2 du GPS : GPS_script2_file "/usr/sbin/lbsappd" */
    /* le script peut être démarrer dans /etc/init.d/lbs -> start, restart, stop, status, force-reload */
    fGPS = fopen(GPS_data_file, "r");
    if(fGPS == NULL)
    {
        printf("failed to open file\n");
    }
    else
    {
            //fscanf(fGPS, "%s",gGPSPacket);
            /*récupère dans un tableau toutes les données du fichier contenant les data GPS */
            while(fgets(gGPSPacket,MAX_GPS_PACKET_SIZE,fGPS) != NULL)
            {
		printf("nlines:%d",nlines++);
                //lit ligne par ligne et cherche le type de trame afin de remplir la structure de données du GPS */
                //parseGPSPacket(gGPSPacket,&gGPSData);
            }
	    nlines=0;
        //}
    }
    fclose(fGPS);
    GPS_data = &gGPSData;
}

/*******************************************************************************
* Function Name  : parseGPSPacket()
* Input          :
* Output         :
* Return         : 1 if successful, 0 otherwise
* Description    :

Parses the GPS packet found in the array 'packet'
This function should be called when a new packet has been received

*******************************************************************************/
int parseGPSPacket( char* packet, GPS_Struct* GPS_data )
{
	int returnval;
	char token[MAX_GPS_PACKET_SIZE];

	// Determine what kind of packet this is
	returnval = getNextGPSToken( packet, token, 1 );

	if( returnval != 1 )
	{
		return 0;
	}

	if( strcmp( token, "$GPGGA" ) == 0 )
	{
		return parseGGA( packet, GPS_data );
	}
	else if( strcmp( token, "$GPGLL" ) == 0 )
	{
		return parseGLL( packet, GPS_data );
	}
	else if( strcmp( token, "$GPGSA" ) == 0 )
	{
		return parseGSA( packet, GPS_data );
	}
	else if( strcmp( token, "$GPGSV" ) == 0 )
	{
		return parseGSV( packet, GPS_data );
	}
	else if( strcmp( token, "$GPRMC" ) == 0 )
	{
		return parseRMC( packet, GPS_data );
	}
	else if( strcmp( token, "$GPVTG" ) == 0 )
	{
		return parseVTG( packet, GPS_data );
	}
	else
	{
		return 0;
	}
}

/*******************************************************************************
* Function Name  : parseGGA( char* token )
* Input          :
* Output         :
* Return         : 1 if successful, 0 otherwise
* Description    :

Parses data in the packet[] buffer as a GGA NMEA message.
This function is designed to be called from within parseGPSPacket().  The
first token in the packet[] buffer should have already been retrieved to
identify the packet type.

FIELDS IN GGA PACKET
1. UTC Time				(hhmmss.sss)
2. Latitude  			(ddmm.mmmm)
3. N/S indicator 		{N,S}
4. Longitude			(dddmm.mmmm)
5. E/W indicator		{E,W}
6. GPS Quality			{0 = position fix unavailable,
						1 = valid position fix, SPS mode,
						2 = valid position fix, differential GPS mode,
						3 = GPS PPS Mode, fix valid
						4 = Real-time Kinematic.  System used in RTK mode.  Floating integers
						5 = Float RTK.  Satellite system used in RTK mode.  Floating integers
						6 = Estimated (dead reckoning) Mode
						7 = Maual Input Mode
						8 = Simulator Mode}
7. Satellites Used		Number of satellites used in solution (00 ~ 12)
8. HDOP					Horizontal Dilution of Precision (00.0 ~ 99.9)
9. Altitude				Altitude above mean sea level (-9999.9 ~ 17999.9)
10 - 13. 				NULL or unsused
14. DGPS Station ID		(0000 ~ 1023, NULL when not used)
15. Checksum

*******************************************************************************/
int parseGGA( char* packet, GPS_Struct* GPS_data )
{
	int returnval;
	char token[MAX_GPS_PACKET_SIZE];

	// 1. Get UTC Time
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	GPS_data->UTC_time = (float)ConvertAtoF( token );

	// 2. Get latitude
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	GPS_data->latitude = (float)ConvertAtoF( token );

	// 3. Get N/S indicator and add to latitude (S = negative, N = positive)
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	if( token[0] == 'N' )
	{
		GPS_data->NS_indicator = token[0];

	}
	else if( token[0] == 'S' )
	{
		GPS_data->NS_indicator = token[0];
		GPS_data->latitude = -GPS_data->latitude;
	}
	else
	{
		return 0;
	}

	// 4. Get longitude
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	GPS_data->longitude = (float)ConvertAtoF( token );

	// 5. Get E/W indicator and add to longitude (W = negative, E = positive)
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	if( token[0] == 'E' )
	{
		GPS_data->EW_indicator = token[0];

	}
	else if( token[0] == 'W' )
	{
		GPS_data->EW_indicator = token[0];
		GPS_data->longitude = -GPS_data->longitude;
	}
	else
	{
		return 0;
	}

	// 6. Get GPS quality indicator
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	GPS_data->quality_indicator = (char)ConvertAtoI( token );

	// 7. Get number of satellites used
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	GPS_data->satellites_used = (char)ConvertAtoI( token );

	// 8. Get HDOP
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	GPS_data->Hdop = (float)ConvertAtoF( token );

	// 9. Get Altitude
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	GPS_data->altitude = (float)ConvertAtoF( token );

	// 10. - 13. iterate through unused fields
	getNextGPSToken( packet, token, 0 );
	getNextGPSToken( packet, token, 0 );
	getNextGPSToken( packet, token, 0 );
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	// 14. Get the DGPS station ID
	returnval = getNextGPSToken( packet, token, 0 );

	if( returnval != 1 )
	{
		return 0;
	}

	if( strlen( token ) > 0 )
	{
		GPS_data->DGPS_station_ID = (int16_t)ConvertAtoI( token );
	}
	else
	{
		GPS_data->DGPS_station_ID = 0;
	}

	// IGNORE CHECKSUM
	// WE MAY WANT TO CHANGE THIS EVENTUALLY TO AVOID ERRORS

	// Set flag indicating that a GGA packet was received and return
	GPS_data->GGA_packet_received = 1;

	return 1;
}

/*******************************************************************************
* Function Name  : parseGLL( char* token )
* Input          :
* Output         :
* Return         : 1 if successful, 0 otherwise
* Description    :

Parses data in the packet[] buffer as a GLL NMEA message.
This function is designed to be called from within parseGPSPacket().  The
first token in the packet[] buffer should have already been retrieved to
identify the packet type.

*******************************************************************************/
int parseGLL( char* packet, GPS_Struct* GPS_data )
{
	return 0;
}

/*******************************************************************************
* Function Name  : parseGSA( char* token )
* Input          :
* Output         :
* Return         : 1 if successful, 0 otherwise
* Description    :

Parses data in the packet[] buffer as a GSA NMEA message.
This function is designed to be called from within parseGPSPacket().  The
first token in the packet[] buffer should have already been retrieved to
identify the packet type.

*******************************************************************************/
int parseGSA( char* packet, GPS_Struct* GPS_data )
{
	return 0;
}

/*******************************************************************************
* Function Name  : parseGSV( char* token )
* Input          :
* Output         :
* Return         : 1 if successful, 0 otherwise
* Description    :

Parses data in the packet[] buffer as a GSV NMEA message.
This function is designed to be called from within parseGPSPacket().  The
first token in the packet[] buffer should have already been retrieved to
identify the packet type.

*******************************************************************************/
int parseGSV( char* packet, GPS_Struct* GPS_data )
{
	return 0;
}

/*******************************************************************************
* Function Name  : parseRMC( char* token )
* Input          :
* Output         :
* Return         : 1 if successful, 0 otherwise
* Description    :

Parses data in the packet[] buffer as a RMC NMEA message.
This function is designed to be called from within parseGPSPacket().  The
first token in the packet[] buffer should have already been retrieved to
identify the packet type.

*******************************************************************************/
int parseRMC( char* packet, GPS_Struct* GPS_data )
{
	return 0;
}

/*******************************************************************************
* Function Name  : parseVTG( char* token )
* Input          :
* Output         :
* Return         : 1 if successful, 0 otherwise
* Description    :

Parses data in the packet[] buffer as a VTG NMEA message.
This function is designed to be called from within parseGPSPacket().  The
first token in the packet[] buffer should have already been retrieved to
identify the packet type.

*******************************************************************************/
int parseVTG( char* packet, GPS_Struct* GPS_data )
{
	return 0;
}

/*******************************************************************************
* Function Name  : getNextGPSToken( char* token, int reset )
* Input          :
* Output         :
* Return         : 1 if successful, 0 otherwise
* Description    :

This function is used to split a received NMEA string into comma-seperated tokens.
If reset != 0, then the tokenizer starts at the beginning and returns the first token.
This function keeps track of what tokens have been extracted, so that if reset == 0,
the next token is returned.

Tokens are seperated by commas, * characters, and <CR><LF> pairs.

Once all tokens have been returned by the function, all subsequent calls to
the function will return 0 until a call has been made with reset == 1

*******************************************************************************/
int getNextGPSToken( char* packet, char* token, int reset )
{
	static int token_start = 0;
	static int no_more_tokens = 0;

	int token_end;

	// If reset flag is set, start tokenizer from beginning
	if( reset )
	{
		token_start = 0;
		no_more_tokens = 0;
	}

	// If all tokens have been read, return 0
	if( no_more_tokens )
	{
		return 0;
	}

	// The token starts at the current index (the index is set so that it immediately follows the last comma)
	// Find the token stop location

	for( token_end = token_start; token_end < MAX_GPS_PACKET_SIZE; token_end++ )
	{
		// If this is end of the token, stop looping immediately (token_end will be used for parsing)
		if( (packet[token_end] == ',') || (packet[token_end] == '*') )
		{
			break;
		}

		// Check for final termination sequence
		if( token_end > 0 )
		{
			if( (packet[token_end] == '\n') && (packet[token_end-1] == '\r') )
			{
				token_end--;
				no_more_tokens = 1;
				break;
			}
		}
	}

	// If token_end is larger than the buffer size, it means that the GPS packet was not terminated correctly (no <CR><LF> pair)
	if( token_end == MAX_GPS_PACKET_SIZE )
	{
		return 0;
	}

	// Copy token to output
	int output_index = 0;
	int i;
	for( i = token_start; i < token_end; i++ )
	{
		token[output_index] = packet[i];
		output_index++;
	}

	// Add null terminating character
	token[output_index] = '\0';

	// Now set the start_index to the first character of the next token
	// token_end has the index of the most recent terminating character, so go 1 index beyond that
	token_start = token_end + 1;

	return 1;
}

/*******************************************************************************
* Function Name  : int ConvertAtoI( char* str )
* Input          :
* Output         :
* Return         : The integer equivalent of the ASCII number representation
* Description    :

This function is used to split a received NMEA string into comma-seperated tokens.
If reset != 0, then the tokenizer starts at the beginning and returns the first token.
This function keeps track of what tokens have been extracted, so that if reset == 0,
the next token is returned.

Tokens are seperated by commas, * characters, and <CR><LF> pairs.

Once all tokens have been returned by the function, all subsequent calls to
the function will return 0 until a call has been made with reset == 1

*******************************************************************************/
int ConvertAtoI( char* str )
{
    return 0;
}

/*******************************************************************************
* Function Name  : float ConvertAtoF( char* str )
* Input          :
* Output         :
* Return         : A float representing the value of the ASCII data
* Description    :

This function is used as an alternative to the standard library function atof(.),
which is way to large to use in this application.

Converts the given string to a floating point number if possible.  If not possible,
returns 0.0

*******************************************************************************/
float ConvertAtoF( char* str )
{
/*
	float a = 0.0;
	int e = 0;
	int c;

	while ((c = *s++) != '\0' && isdigit(c)) {
			a = a*10.0 + (c - '0');
	}
	if (c == '.') {
			while ((c = *s++) != '\0' && isdigit(c)) {
					a = a*10.0 + (c - '0');
					e = e-1;
			}
	}
	if (c == 'e' || c == 'E') {
			int sign = 1;
			int i = 0;
			c = *s++;
			if (c == '+')
					c = *s++;
			else if (c == '-') {
					c = *s++;
					sign = -1;
			}
			while (isdigit(c)) {
					i = i*10 + (c - '0');
					c = *s++;
			}
			e += i*sign;
	}
	while (e > 0) {
			a *= 10.0;
			e--;
	}
	while (e < 0) {
			a *= 0.1;
			e++;
	}
	return a;
	*/

	int index;
	int decimal_location;
	int str_length;
	char negative;
	float output;

	// Get the length of the string, and set the default period location
	// If there is no period, the period location should be assumed to be
	// at the end of the string, where it technically would be if there was one.
	// (i.e. the number 1010 is the same as 1010.00)
	str_length = strlen( str );
	decimal_location = str_length;

	// Step one: find the comma (if there is one)
	for( index = 0; index < str_length; index++ )
	{
		if( str[index] == '.' )
		{
			decimal_location = index;
			break;
		}
	}

	// Step two: determine whether this is a negative number or not
	if( str[0] == '-' )
	{
		negative = 1;
		index = 1;
	}
	else
	{
		negative = 0;
		index = 0;
	}

	// Step three: iterate through everything prior to the comma location and compute values
	// Only values 0 through 9 are accepted.  The ASCII codes for characters 0 through 9 range from 48 to 57, respectively.
	// To compute the actual value of the ascii character based on its code, simply subtract 48 from the value.
	output = 0;
	for( ; index < decimal_location; index++ )
	{
		char number;

		// Compute the actual value of this character, or stop if it is invalid
		if( (str[index] <= 57) && (str[index] >= 48) )
		{
			number = str[index] - 48;
		}
		else
		{
			return 0.0;
		}

		// Compute the value of the current location and add to the total
		char power = (decimal_location - index - 1);
		output += (float)(uint32Power( 10, (uint32_t)power )*number);
	}

	// Step four: Iterate through everything after the decimal location and compute values
	for( index = (decimal_location+1); index < str_length; index++ )
	{
		char number;

		// Compute the actual value of this character, or stop if it is invalid
		if( (str[index] <= 57) && (str[index] >= 48) )
		{
			number = str[index] - 48;
		}
		else
		{
			return 0.0;
		}

		// Compute the value of the current location and add to the total
		char power = (index - decimal_location);
		output += (float)((1.0/(float)uint32Power( 10, (uint32_t)power ))*(float)number);
	}

	// Step five: apply negative sign if there is one
	if( negative )
	{
		output = -output;
	}

	return output;

}


/*******************************************************************************
* Function Name  : uint32_t uint32Power( uint32_t x, uint32_t y )
* Input          :
* Output         :
* Return         : The value x^y
* Description    :

This function is used instead of the math.h pow() function.  We only need powers
of unisgned integers, not of doubles.  This function does the job using much less
space.

*******************************************************************************/
uint32_t uint32Power( uint32_t x, uint32_t y )
{
	int index;
	uint32_t result;

	if( y == 0 )
	{
		return 1;
	}

	result = x;
	for( index = 0; index < (y-1); index++ )
	{
		result = result*x;
	}

	return result;
}
